#pragma once
#include <cstdint>
#include <vector>
#include <string>
#include <array>
#include <memory>
#include "zuc/nml_intf/zucpos2.h"
#include "zuc/mot_cfg.h"

namespace Util {
class IniParser;
}

namespace axisgroup {

class ToolData
{
public:
    ToolData(int id = 0);
    void reset();
    const ZucPose& pose();
    std::string name();
    void config_name(std::string n);

    /**
     * @brief 获取工具的数据源
     * @return int 0-手动设置 1-4点标定 2-6点标定
     */
    virtual int data_source();

public:  //手动设置
    /**
     * @brief 设置工具信息
     * @param pose 工具坐标
     * @param save 保存信息到配置文件
     */
    virtual int set_pose(ZucPose pose, bool save = false);

public:  //工具标定
    struct CalibData
    {
        bool confirmed;
        std::vector<ZucPose> pose;     //FIXME:理论上需要再加上refjoint或jointconf才行
        double eps, eps_max, eps_min;  //标定误差
    };
    virtual int calc_tool_pose(std::array<ZucPose, 4> pose, CalibData* result);
    virtual int calc_tool_pose(std::array<ZucPose, 6> pose, CalibData* result);
    virtual CalibData get_calib_data();
    virtual int confirm_calib_result(CalibData* result = nullptr);

private:
    ZucPose pose_;
    std::string name_;
    CalibData calib;
    uint8_t id_;
    bool is_changed_ = false;

    friend class RobotToolMgr;
};

class RobotToolMgr
{
public:
    RobotToolMgr() = delete;
    const static uint32_t MAX_ROBOT_TOOL_NUM = kRobToolMaxCnt;
    static ToolData& get_rob_tool(uint32_t id);
    static int init_rob_tool(const char* filename);
    static void save();

private:
    int id_ = 0;
    static std::unique_ptr<Util::IniParser> ini_parser_;
    static ToolData __robot_tool[MAX_ROBOT_TOOL_NUM];
};

}  // namespace axisgroup
